/*
 **Ball and Plate program**
 
 This program runs the ball and plate experiment. For more information, see:
 
 The usage is
 "bandp init" initializes the matrices.
 "bandp run" runs the program once the matrices have already been initialized.
 "bandp" initializes the matrices and then runs the program. This is the equivalent to first running "bandp init" and then "bandp run". 
 
 */


#include <OpenCV/OpenCV.h>
#include <iostream>
#include "init.h"
#include "run.h"

#define DELTA_T .040 // Sample time in [s]
#define STATE_DIM 4 // Number of state dimensions
#define SAVED_PAST 1.0 / DELTA_T // Number of seconds of previous inputs and outputs we keep in memory

int flag;

int main (int argc, char * const argv[]) 
{
	if (argc==1){
		printf("Init finished successfully.\n");	
		flag=run();  // run program
	}
	
	if ( strcmp(argv[1],"init")==0){
		//Measure time
		int64 tic=cvGetTickCount();
		double ticRate = cvGetTickFrequency()*1e06;
		//Loop exists just to see how fast the program is.
		for (int i=1; i <2; i++)
		{
			
			init(); // Generate init file
			printf("Init finished successfully.\n");	
			
			
		}
		//Measure time
		int64 toc = cvGetTickCount();
		printf("\nComputation time: %lf seconds\n", (double) (toc-tic) / ticRate );
	}
	else if( strcmp(argv[1],"run")==0)
	{
		// Read init file
		// Check flag
		/*			if (!flag)
		 {
		 std::cout << "The matrices have not been initialized. Please run \"bandp init\" first" << std::endl; 
		 return 1;
		 }
		 */			
		flag=run(); // Run program
	}
	else if( strcmp(argv[1],"help")==0)
	{
		printf("\n");	
		printf("bandp init : intializes program.\n");	
		printf("bandp run  : runs program. Requires init to have been run previously\n");	
		printf("bandp      : intializes and then runs program.\n");	
		printf("\n");	
	}
	return 0;
}